/**
 * 
 */
package dijkstra;

import roborally.model.*;
import java.util.*;

import exception.IllegalPositionException;

/**
 * @author r0258417
 *
 */
public class BoardGraphConverter {
	
	public BoardGraphConverter( Robot robot) {
		positionList = new ArrayList<Position>();
		graph = new EdgeWeightedDigraph(0);
		this.robot = robot;
		try {
			oldPosition = robot.getPosition();
			oldOrientation = robot.getOrientation();
			graphForRobot();
		}
		catch(NullPointerException e) {
			
		}
		
	}
	
	public Range getRobotRange() {
		
		Range range = new Range();
		if(!robot.getPosition().equals(oldPosition) || !robot.getOrientation().equals(oldOrientation)){
			graphForRobot();
		}
		for(Position position : positionList) {
		
			range.addPosition(position);
		}
		return range;
	}
	
	public double getEnergyToReach(Position position){
		
		if( !robot.getPosition().equals(oldPosition) || !robot.getOrientation().equals(oldOrientation)){
			graphForRobot();
		}
		DijkstraSP dijkstra = new DijkstraSP(graph, robot.getOrientation().intValue());
		double result = Double.MAX_VALUE;
		for(int i = 0; i < 4; i++) {
			result = result > dijkstra.distTo(positionList.indexOf(position)*4 + i) ? dijkstra.distTo(positionList.indexOf(position)*4 + i) : result;
		}
		
		return result;
	}
	
	public Orientation getPostMoveOrientation(Position position){
		
		if(!robot.getPosition().equals(oldPosition) || !robot.getOrientation().equals(oldOrientation)){
			graphForRobot();
		}
		DijkstraSP dijkstra = new DijkstraSP(graph, robot.getOrientation().intValue());
		double minEnergy = Double.MAX_VALUE;
		int index = 0;
		for(int i = 0; i < 4; i++) {
			if(minEnergy > dijkstra.distTo(positionList.indexOf(position)*4 + i)){
				minEnergy = dijkstra.distTo(positionList.indexOf(position)*4 + i);
				index = i;
			}
		}
		Orientation result = Orientation.UP;
		for(Orientation or : Orientation.values()){
			if(or.intValue() == index){
				result = or;
			}
		}
		return result;
	}
	
	private void graphForRobot() {
		
		//setup the graph and the list of positions
		//List<Position> positionList = new ArrayList<Position>();
		positionList.clear();
		
		oldPosition = robot.getPosition();
		oldOrientation = robot.getOrientation();
		
		positionList.add(robot.getPosition());
		graph = new EdgeWeightedDigraph(Orientation.values().length);
		for(int i=0;i<Orientation.values().length; i++){
			for(int j=0;j<Orientation.values().length; j++){
				int numberOfTurns;
				numberOfTurns = Math.abs(i-j)>0 ? (Math.abs(i-j)==2 ? 2 : 1) : 0;
				graph.addEdge(new DirectedEdge(i,j,Robot.getEnergyRequiredToTurn().getValueIn(Energy.Unit.WS)*numberOfTurns));
			}
		}
		
		//keep adding positions based on the current position list until the list stops changing
		//for(Position position : positionList){
		for(int p = 0; p < positionList.size(); p++){
			Position position = positionList.get(p);
			
			for(Orientation orientation : Orientation.values()){
				boolean cantShare = false;
				try {
					for(Element element : robot.getBoard().getElements(position.getNeighbour(orientation))){
						if(!cantShare) {
							cantShare = !robot.canSharePositionWith(element);
						}
					}
				}
				catch(IllegalPositionException e) {
					
				}
				
				DijkstraSP dijkstra = new DijkstraSP(graph, robot.getOrientation().intValue());
				//DijkstraSP dijkstra = new DijkstraSP(graph, 0);
				//if the next position is not a wall and the robot has enough energy to reach this position
				if(!cantShare && dijkstra.distTo(positionList.indexOf(position)*4+orientation.intValue()) + Robot.getEnergyRequiredToMove().getValueIn(Energy.Unit.WS)+ robot.getLoad()*Robot.getLoadFactor() <= robot.getEnergy().getValueIn(Energy.Unit.WS)){
					EdgeWeightedDigraph tempGraph;
					//if the next position does not already exists in the list (not added yet)
					try {
						if(!positionList.contains(position.getNeighbour(orientation))){
							//add the next position in the list
							positionList.add(position.getNeighbour(orientation));
							//add the next position in a temporary graph
							tempGraph = new EdgeWeightedDigraph(graph.V()+4,graph);
							//add the edges between the new position
							for(int i=0;i<Orientation.values().length; i++){
								for(int j=0;j<Orientation.values().length; j++){
									int numberOfTurns;
									numberOfTurns = Math.abs(i-j)>0 ? (Math.abs(i-j)==2 ? 2 : 1) : 0;
									tempGraph.addEdge(new DirectedEdge((positionList.size()-1)*4+i,(positionList.size()-1)*4+j,Robot.getEnergyRequiredToTurn().getValueIn(Energy.Unit.WS)*numberOfTurns));
								}
							}
						}
						else{
							tempGraph = new EdgeWeightedDigraph(graph);
						}
					}
					catch(IllegalPositionException e) {
						tempGraph = new EdgeWeightedDigraph(graph);
					}
					try {
						//add a new edge between the current position and the next position
					tempGraph.addEdge(new DirectedEdge(positionList.indexOf(position)*4+orientation.intValue(),
							positionList.indexOf(position.getNeighbour(orientation))*4+orientation.intValue(),
							Robot.getEnergyRequiredToMove().getValueIn(Energy.Unit.WS)+ robot.getLoad()*Robot.getLoadFactor()));
					
					graph = new EdgeWeightedDigraph(tempGraph);
					}
					catch(IllegalPositionException e) {
						graph = new EdgeWeightedDigraph(tempGraph);
					}
					
				}
			}
		}
	}
	
	private List<Position> positionList;
	private EdgeWeightedDigraph graph;
	private Robot robot;
	
	private Position oldPosition;
	private Orientation oldOrientation;

}
